/*
 * PID.c
 *
 *  Created on: Jun 10, 2024
 *      Author: Liangmiyi
 */

#include <MY_INC/PHY/PHY_SVPWM.h>

/*==================================�߶�ʽSVPWM�㷨 ����һ===================================*/
/*
 * ���ƣ��߶�ʽSVPWM����
 * ���ܣ������߶�ʽSVPWM���õ�ABC����������ȣ���ӦMatlab����
 * ���������
 *     uint32_t Pulse_width_HRTIM[3]    ������������ 0-A�� 1-B�� 2-C��
 *     float Vref[3]                    ��ABC��ο���ѹ
 *     float Udc                        ��ֱ�����ѹ
 *     uint32_t PWM_period              ���������ڿ��
 * ���ز�������
 */
void Three_Phase_Inverter_SVPWM_Seven_1(uint32_t Pulse_width_HRTIM[3], float Vref[3], const uint32_t PWM_period)
{

    int32_t ua = (int32_t)(Vref[0] * PWM_period); // 中间变量
    int32_t ub = (int32_t)(Vref[1] * PWM_period);
    int32_t uc = (int32_t)(Vref[2] * PWM_period);                             // ֵ
    int32_t umax = (ua > ub) ? ((uc > ua) ? uc : ua) : ((uc > ub) ? uc : ub); // 找到最大值ֵ
    int32_t umin = (ua < ub) ? ((uc < ua) ? uc : ua) : ((uc < ub) ? uc : ub); // 找到最小值
    int32_t uzs = -((umax + umin) >> 1);

    Pulse_width_HRTIM[0] = (uint32_t)(ua + uzs + (PWM_period >> 1));
    Pulse_width_HRTIM[1] = (uint32_t)(ub + uzs + (PWM_period >> 1));
    Pulse_width_HRTIM[2] = (uint32_t)(uc + uzs + (PWM_period >> 1));

    Pulse_width_HRTIM[0] = (Pulse_width_HRTIM[0] > U_LIMIT_HIGH) ? U_LIMIT_HIGH : Pulse_width_HRTIM[0];
    Pulse_width_HRTIM[0] = (Pulse_width_HRTIM[0] < U_LIMIT_ZERO) ? 0 : Pulse_width_HRTIM[0];
    Pulse_width_HRTIM[1] = (Pulse_width_HRTIM[1] > U_LIMIT_HIGH) ? U_LIMIT_HIGH : Pulse_width_HRTIM[1];
    Pulse_width_HRTIM[1] = (Pulse_width_HRTIM[1] < U_LIMIT_ZERO) ? 0 : Pulse_width_HRTIM[1];
    Pulse_width_HRTIM[2] = (Pulse_width_HRTIM[2] > U_LIMIT_HIGH) ? U_LIMIT_HIGH : Pulse_width_HRTIM[2];
    Pulse_width_HRTIM[2] = (Pulse_width_HRTIM[2] < U_LIMIT_ZERO) ? 0 : Pulse_width_HRTIM[2];

    //    int32_t ua = (int32_t)(Ut[0] * PWM_PERIOD ); // 中间变量
    //    int32_t ub = (int32_t)(Ut[1] * PWM_PERIOD );
    //    int32_t uc = (int32_t)(Ut[2] * PWM_PERIOD ); // ֵ
    //    int32_t umax = (ua > ub) ? ((uc > ua) ? uc : ua) : ((uc > ub) ? uc : ub); // 找到最大值ֵ
    //    int32_t umin = (ua < ub) ? ((uc < ua) ? uc : ua) : ((uc < ub) ? uc : ub); // 找到最小值
    //    int32_t uzs = -((umax + umin) >> 1);
    //
    //    Pulse_width_HRTIM[0] = (uint32_t)(ua + uzs + (PWM_PERIOD >> 1));
    //    Pulse_width_HRTIM[1] = (uint32_t)(ub + uzs + (PWM_PERIOD >> 1));
    //    Pulse_width_HRTIM[2] = (uint32_t)(uc + uzs + (PWM_PERIOD >> 1));
    //
    //    Pulse_width_HRTIM[0] = (Pulse_width_HRTIM[0] > U_LIMIT_HIGH) ? U_LIMIT_HIGH : Pulse_width_HRTIM[0];
    //    Pulse_width_HRTIM[0] = (Pulse_width_HRTIM[0] < U_LIMIT_ZERO) ? 0 : Pulse_width_HRTIM[0];
    //    Pulse_width_HRTIM[1] = (Pulse_width_HRTIM[1] > U_LIMIT_HIGH) ? U_LIMIT_HIGH : Pulse_width_HRTIM[1];
    //    Pulse_width_HRTIM[1] = (Pulse_width_HRTIM[1] < U_LIMIT_ZERO) ? 0 : Pulse_width_HRTIM[1];
    //    Pulse_width_HRTIM[2] = (Pulse_width_HRTIM[2] > U_LIMIT_HIGH) ? U_LIMIT_HIGH : Pulse_width_HRTIM[2];
    //    Pulse_width_HRTIM[2] = (Pulse_width_HRTIM[2] < U_LIMIT_ZERO) ? 0 : Pulse_width_HRTIM[2];
}

/*==================================�߶�ʽSVPWM�㷨 ������===================================*/
/*
 * ���ƣ��߶�ʽSVPWM����
 * ���ܣ������߶�ʽSVPWM���õ�ABC�����������
 * ���������
 *     uint32_t Pulse_width_HRTIM[3]    ������������ 0-A�� 1-B�� 2-C��
 *     float Vref[3]                    ��ABC��ο���ѹ
 *     float Udc                        ��ֱ�����ѹ
 *     uint32_t PWM_period              ���������ڿ��
 * ���ز�������
 */
void Three_Phase_Inverter_SVPWM_Seven_2(uint32_t Pulse_width_HRTIM[3], float Vref[3], float Udc,
                                        const uint32_t PWM_period)
{
    int32_t ua, ub, uc; // �м����
    int32_t uzs;        // �������

    ua = (int32_t)((Vref[0] / Udc) * PWM_period);
    ub = (int32_t)((Vref[1] / Udc) * PWM_period);
    uc = (int32_t)((Vref[2] / Udc) * PWM_period);

    if ((ua >= ub && ua <= uc) || (ua <= ub && ua >= uc))
        uzs = ua >> 1; // a����λ��
    else if ((ub >= ua && ub <= uc) || (ub <= ua && ub >= uc))
        uzs = ub >> 1; // b����λ��
    else
        uzs = uc >> 1; // c����λ��

    Pulse_width_HRTIM[0] = (uint32_t)(ua + uzs + (PWM_period >> 1));
    Pulse_width_HRTIM[1] = (uint32_t)(ub + uzs + (PWM_period >> 1));
    Pulse_width_HRTIM[2] = (uint32_t)(uc + uzs + (PWM_period >> 1));

    if (Pulse_width_HRTIM[0] > U_LIMIT_HIGH)
        Pulse_width_HRTIM[0] = U_LIMIT_HIGH;
    else if (Pulse_width_HRTIM[0] < 0)
        Pulse_width_HRTIM[0] = 0;

    if (Pulse_width_HRTIM[1] > U_LIMIT_HIGH)
        Pulse_width_HRTIM[1] = U_LIMIT_HIGH;
    else if (Pulse_width_HRTIM[1] < 0)
        Pulse_width_HRTIM[1] = 0;

    if (Pulse_width_HRTIM[2] > U_LIMIT_HIGH)
        Pulse_width_HRTIM[2] = U_LIMIT_HIGH;
    else if (Pulse_width_HRTIM[2] < 0)
        Pulse_width_HRTIM[2] = 0;
}

/*==================================���ʽSVPWM�㷨 ����һ===================================*/
/*
 * ���ƣ����ʽSVPWM��ͺ���
 * ���ܣ��������ʽSVPWM���õ�ABC�����������
 * ���������
 *     uint32_t Pulse_width_HRTIM[3]    ������������ 0-A�� 1-B�� 2-C��
 *     float Vref[3]                    ��ABC��ο���ѹ
 *     float Udc                        ��ֱ�����ѹ
 *     uint32_t PWM_period              ���������ڿ��
 * ���ز�������
 */
void Three_Phase_Inverter_SVPWM_Five_Low(uint32_t Pulse_width_HRTIM[3], float Vref[3], const uint32_t PWM_period)
{

    int32_t ua = (int32_t)((Vref[0]) * PWM_period); // 中间变量
    int32_t ub = (int32_t)((Vref[1]) * PWM_period);
    int32_t uc = (int32_t)((Vref[2]) * PWM_period);

    // 找到最小值
    int32_t umin = (ua < ub) ? ((uc < ua) ? uc : ua) : ((uc < ub) ? uc : ub); // 最小值ֵ

    Pulse_width_HRTIM[0] = (uint32_t)(ua - umin);
    Pulse_width_HRTIM[1] = (uint32_t)(ub - umin);
    Pulse_width_HRTIM[2] = (uint32_t)(uc - umin);

    Pulse_width_HRTIM[0] = (Pulse_width_HRTIM[0] > U_LIMIT_HIGH) ? U_LIMIT_HIGH : Pulse_width_HRTIM[0];
    Pulse_width_HRTIM[0] = (Pulse_width_HRTIM[0] < U_LIMIT_ZERO) ? 0 : Pulse_width_HRTIM[0];
    Pulse_width_HRTIM[1] = (Pulse_width_HRTIM[1] > U_LIMIT_HIGH) ? U_LIMIT_HIGH : Pulse_width_HRTIM[1];
    Pulse_width_HRTIM[1] = (Pulse_width_HRTIM[1] < U_LIMIT_ZERO) ? 0 : Pulse_width_HRTIM[1];
    Pulse_width_HRTIM[2] = (Pulse_width_HRTIM[2] > U_LIMIT_HIGH) ? U_LIMIT_HIGH : Pulse_width_HRTIM[2];
    Pulse_width_HRTIM[2] = (Pulse_width_HRTIM[2] < U_LIMIT_ZERO) ? 0 : Pulse_width_HRTIM[2];
}

/*==================================���ʽSVPWM�㷨 ������ ���Ƽ�ʹ��===================================*/
/*
 * ���ƣ����ʽSVPWM��ߺ���
 * ���ܣ��������ʽSVPWM���õ�ABC�����������
 * ���������
 *     uint32_t Pulse_width_HRTIM[3]    ������������ 0-A�� 1-B�� 2-C��
 *     float Vref[3]                    ��ABC��ο���ѹ
 *     float Udc                        ��ֱ�����ѹ
 *     uint32_t PWM_period              ���������ڿ��
 * ���ز�������
 */
void Three_Phase_Inverter_SVPWM_Five_High(uint32_t Pulse_width_HRTIM[3], float Vref[3], float Udc,
                                          const uint32_t PWM_period)
{
    int32_t ua, ub, uc; // �м����
    int32_t umax;       // ���ֵ����

    ua = (int32_t)((Vref[0] / Udc) * PWM_period);
    ub = (int32_t)((Vref[1] / Udc) * PWM_period);
    uc = (int32_t)((Vref[2] / Udc) * PWM_period);

    // �ҳ������������ֵ
    if (ua > ub)
        umax = ua;
    else
        umax = ub;
    if (umax < uc)
        umax = uc;

    Pulse_width_HRTIM[0] = (uint32_t)(ua - umax + PWM_period);
    Pulse_width_HRTIM[1] = (uint32_t)(ub - umax + PWM_period);
    Pulse_width_HRTIM[2] = (uint32_t)(uc - umax + PWM_period);

    if (Pulse_width_HRTIM[0] > PWM_period)
        Pulse_width_HRTIM[0] = PWM_period;
    else if (Pulse_width_HRTIM[0] < U_LIMIT_LOW)
        Pulse_width_HRTIM[0] = U_LIMIT_LOW;

    if (Pulse_width_HRTIM[1] > PWM_period)
        Pulse_width_HRTIM[1] = PWM_period;
    else if (Pulse_width_HRTIM[1] < U_LIMIT_LOW)
        Pulse_width_HRTIM[1] = U_LIMIT_LOW;

    if (Pulse_width_HRTIM[2] > PWM_period)
        Pulse_width_HRTIM[2] = PWM_period;
    else if (Pulse_width_HRTIM[2] < U_LIMIT_LOW)
        Pulse_width_HRTIM[2] = U_LIMIT_LOW;

    Pulse_width_HRTIM[0] -= 1; // ��ֹ����ֵ���������ת
    Pulse_width_HRTIM[1] -= 1;
    Pulse_width_HRTIM[2] -= 1;
}

/*==================================˫����SPWM�㷨 ���Ƽ�ʹ��===================================*/
/*
 * ���ƣ�˫����SPWM�㷨
 * ���ܣ�����˫����SPWM���õ�ABC�����������
 * ���������
 *     uint32_t Pulse_width_HRTIM[3]    ������������ 0-A�� 1-B�� 2-C��
 *     float Vref[3]                    ��ABC��ο���ѹ
 *     float Udc                        ��ֱ�����ѹ
 *     uint32_t PWM_period              ���������ڿ��
 * ���ز�������
 */
void Three_Phase_Inverter_SPWM(uint32_t Pulse_width_HRTIM[3], float Vref[3], float Udc, const uint32_t PWM_period)
{

    Pulse_width_HRTIM[0] = (uint32_t)((Vref[0] / Udc) * PWM_period) + (PWM_period >> 1);
    Pulse_width_HRTIM[1] = (uint32_t)((Vref[1] / Udc) * PWM_period) + (PWM_period >> 1);
    Pulse_width_HRTIM[2] = (uint32_t)((Vref[2] / Udc) * PWM_period) + (PWM_period >> 1);

    if (Pulse_width_HRTIM[0] > U_LIMIT_HIGH)
        Pulse_width_HRTIM[0] = U_LIMIT_HIGH;
    else if (Pulse_width_HRTIM[0] < 0)
        Pulse_width_HRTIM[0] = 0;

    if (Pulse_width_HRTIM[1] > U_LIMIT_HIGH)
        Pulse_width_HRTIM[1] = U_LIMIT_HIGH;
    else if (Pulse_width_HRTIM[1] < 0)
        Pulse_width_HRTIM[1] = 0;

    if (Pulse_width_HRTIM[2] > U_LIMIT_HIGH)
        Pulse_width_HRTIM[2] = U_LIMIT_HIGH;
    else if (Pulse_width_HRTIM[2] < 0)
        Pulse_width_HRTIM[2] = 0;
}
